#include "pwm_ctrl.h"

uint16_t Angle_Set(uint8_t angle)
{
	return angle * 2000 / 180 + 500;
}

void Rbt_Init(void)
{
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(90));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(90));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(90));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(90));	
}

void Rbt_Move_Forward(void)
{
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(135));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(45));	
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(45));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(135));
	
	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(90));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(90));	
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(90));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(90));

	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(135));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(45));
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(45));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(135));
	
	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(90));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(90));
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(90));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(90));	
}

void Rbt_Move_Behind(void)
{
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(45));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(135));	
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(135));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(45));
	
	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(90));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(90));	
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(90));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(90));

	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(45));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(135));
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(135));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(45));
	
	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(90));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(90));
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(90));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(90));	
}

void Rbt_Move_Right(void)
{
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(45));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(45));	
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(135));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(135));
	
	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(90));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(90));	
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(90));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(90));

	HAL_Delay(MOVE_DELAY);
}

void Rbt_Move_Left(void)
{
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(135));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(135));
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(45));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(45));
	
	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(90));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(90));
	HAL_Delay(MOVE_DELAY);
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(90));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(90));	

	HAL_Delay(MOVE_DELAY);
}

void Rbt_Move_Swing(void)
{
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(130));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(130));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(50));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(50));
	
	HAL_Delay(MOVE_DELAY);
	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(50));	
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(50));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(130));
	__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(130));	

	HAL_Delay(MOVE_DELAY);
}

void Rbt_Sleep(void)
{
	if(TIM2->CCR3 < Angle_Set(160) && TIM2->CCR3 > Angle_Set(20)){
		for(uint8_t i = 0; i < 75; i++){
			__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_1,Angle_Set(90 - i));
			__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,Angle_Set(90 + i));
		}
	}
	if(TIM2->CCR2 < Angle_Set(160) && TIM4->CCR3 > Angle_Set(20)){
		for(uint8_t i = 0; i < 75; i++){
			__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,Angle_Set(90 + i));
			__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,Angle_Set(90 - i));
		}
	}
}


































